//
// Created by root on 22-10-24.
//
#include <iostream>
#include <vector>
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <thread>
#include "visualInertialOdometry.h"
#include "Estimator.h"
#include "Parameters.h"

using namespace std;
using namespace cv;

VisualInertialOdometry::VIO *vio;
Estimator *estimator;


void img_callback(const sensor_msgs::ImageConstPtr &img_msg){
    vio->processImage(img_msg);
}

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg){
    estimator->processIMU(imu_msg);
}

void feature_callback(const sensor_msgs::PointCloudPtr &feature_msg){
    estimator->processFeature(feature_msg);
}


void SLAM(){

    //1、Initialize

    //2、feature_tracker
    thread* th_vio=new thread(&VisualInertialOdometry::VIO::process,vio);

    //3、estimator
    thread* th_esti=new thread(&Estimator::process,estimator);

    //4、Optimier



}


int main(int argc,char **argv){
    ros::init(argc, argv, "VINS2ORB");
    ros::NodeHandle n("~");
    parametersInitialize();
    ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100,img_callback);
    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC,100,imu_callback);
    ros::Subscriber sub_feature = n.subscribe(FEATURE_TOPIC,100,feature_callback);
    vio=new VisualInertialOdometry::VIO();
    estimator=new Estimator();

    SLAM();
    ros::spin();
    return 0;
}
